#include <string.h>
#include <stdio.h>
#include <math.h>
#include "pid_fuzzy.h"


#define PWM_PIN_ON()               
#define PWM_PIN_OFF()    


#define NB -6
#define NM -4
#define NS -2
#define ZO 0
#define PS 2
#define PM 4
#define PB 6
#define     MAX_VALUE_ERROR               4.0
#define     MIN_VALUE_ERROR              -4.0


#define PWM_MAX_VALUE  100
#define PWM_GAIN           30  //  = (PWM_MAX_VALUE /(PV-SV))
uint32_t ctltime = 500;   //控制周期500ms
uint32_t pwm_on_time = 0;
uint32_t pwm_0ff_time = 500;
uint32_t lastsystime = 0;
uint32_t pwmcount;
uint8_t  automodule = 1; //手动控制PWM输出
uint8_t  pwmoutvalue =0;


FUZZYPID  pid;
int ruleKp[7][7] = {{PB, PB, PM, PM, PS, ZO, ZO},
					{PB, PB, PM, PS, PS, ZO, ZO},
					{PM, PM, PM, PS, ZO, NS, NS},
					{PM, PM, PS, ZO, NS, NM, NM},
					{PS, PS, ZO, NS, NS, NM, NM},
					{PS, ZO, NS, NM, NM, NM, NB},
					{ZO, ZO, NM, NM, NM, NB, NB}};

int ruleKd[7][7] = {{PS, NS, NB, NB, NB, NM, PS},
					{PS, NS, NB, NM, NM, NS, ZO},
					{ZO, NS, NM, NM, NS, NS, ZO},
					{ZO, NS, NS, NS, NS, NS, ZO},
					{ZO, ZO, ZO, ZO, ZO, ZO, ZO},
					{PB, NS, PS, PS, PS, PS, PB},
					{PB, PM, PM, PM, PS, PS, PB}};

int ruleKi[7][7] = {{NB, NB, NM, NM, NS, ZO, ZO},
					{NB, NB, NM, NS, NS, ZO, ZO},
					{NB, NM, NS, NS, ZO, PS, PS},
					{NM, NM, NS, ZO, PS, PM, PM},
					{NM, NS, ZO, PS, PS, PM, PB},
					{ZO, ZO, PS, PS, PM, PB, PB},
					{ZO, ZO, PS, PM, PM, PB, PB}};

float LinearRealization(float maximum, float minimum, float value)
{
    
    float x = (maximum - minimum) *(value )/6;
	return x;

}


static void LinearQuantization(FUZZYPID *vPID, float pv, float *qValue)
{
	float thisError;
	float deltaError;

	thisError = vPID->setpoint - pv;		  
	deltaError = thisError - vPID->lasterror; 

	qValue[0] = 6.0 * thisError / (vPID->maximum - vPID->minimum);
	qValue[1] = 3.0 * deltaError / (vPID->maximum - vPID->minimum);
}


static void CalcMembership(float *ms, float qv, int *index)
{
	if ((qv >= -NB) && (qv < -NM))
	{
		index[0] = 0;
		index[1] = 1;
		ms[0] = -0.5 * qv - 2.0; // y=-0.5x-2.0
		ms[1] = 0.5 * qv + 3.0;	 // y=0.5x+3.0
	}
	else if ((qv >= -NM) && (qv < -NS))
	{
		index[0] = 1;
		index[1] = 2;
		ms[0] = -0.5 * qv - 1.0; // y=-0.5x-1.0
		ms[1] = 0.5 * qv + 2.0;	 // y=0.5x+2.0
	}
	else if ((qv >= -NS) && (qv < ZO))
	{
		index[0] = 2;
		index[1] = 3;
		ms[0] = -0.5 * qv;		// y=-0.5x
		ms[1] = 0.5 * qv + 1.0; // y=0.5x+1.0
	}
	else if ((qv >= ZO) && (qv < PS))
	{
		index[0] = 3;
		index[1] = 4;
		ms[0] = -0.5 * qv + 1.0; // y=-0.5x+1.0
		ms[1] = 0.5 * qv;		 // y=0.5x
	}
	else if ((qv >= PS) && (qv < PM))
	{
		index[0] = 4;
		index[1] = 5;
		ms[0] = -0.5 * qv + 2.0; // y=-0.5x+2.0
		ms[1] = 0.5 * qv - 1.0;	 // y=0.5x-1.0
	}
	else if ((qv >= PM) && (qv <= PB))
	{
		index[0] = 5;
		index[1] = 6;
		ms[0] = -0.5 * qv + 3.0; // y=-0.5x+3.0
		ms[1] = 0.5 * qv - 2.0;	 // y=0.5x-2.0
	}
}

/*��ģ��������,���ݾ�������������������Ⱥ�������*/
static void FuzzyComputation(FUZZYPID *vPID, float pv, float *deltaK)
{
	float qValue[2] = {0, 0}; // ƫ�������������ֵ
	int indexE[2] = {0, 0};	  // ƫ������������
	float msE[2] = {0, 0};	  // ƫ��������
	int indexEC[2] = {0, 0};  // ƫ����������������
	float msEC[2] = {0, 0};	  // ƫ������������
	float qValueK[3];

	LinearQuantization(vPID, pv, qValue);

	CalcMembership(msE, qValue[0], indexE);
	CalcMembership(msEC, qValue[1], indexEC);

    qValueK[0] = msE[0] * (msEC[0] * ruleKp[indexE[0]][indexEC[0]] + msEC[1] * ruleKp[indexE[0]][indexEC[1]]) 
           + msE[1] * (msEC[0] * ruleKp[indexE[1]][indexEC[0]] + msEC[1] * ruleKp[indexE[1]][indexEC[1]]);

    qValueK[1] = msE[0] * (msEC[0] * ruleKi[indexE[0]][indexEC[0]] + msEC[1] * ruleKi[indexE[0]][indexEC[1]]) 
           + msE[1] * (msEC[0] * ruleKi[indexE[1]][indexEC[0]] + msEC[1] * ruleKi[indexE[1]][indexEC[1]]);

    qValueK[2] = msE[0] * (msEC[0] * ruleKd[indexE[0]][indexEC[0]] + msEC[1] * ruleKd[indexE[0]][indexEC[1]]) 
           + msE[1] * (msEC[0] * ruleKd[indexE[1]][indexEC[0]] + msEC[1] * ruleKd[indexE[1]][indexEC[1]]);

  
	vPID->deltaKp = LinearRealization(vPID->maxdKp, vPID->mindKp, qValueK[0]);
	vPID->deltaKi = LinearRealization(vPID->maxdKi, vPID->mindKi, qValueK[1]);
	vPID->deltaKd = LinearRealization(vPID->maxdKd, vPID->mindKd, qValueK[2]);
    
    deltaK[0] = vPID->deltaKp + vPID->kp;
	deltaK[1] = vPID->deltaKi + vPID->ki;
	deltaK[2] = vPID->deltaKd + vPID->kd;

}


void PIDIint()
{
    FUZZYPID *vPID = &pid;
     stc_port_init_t stcPortInit;
	vPID->kp = 1;
	vPID->ki = 0.1;
	vPID->kd = 0;
	vPID->maximum = MAX_VALUE_ERROR;
	vPID->minimum = MIN_VALUE_ERROR;
	vPID->maxdKp = 2;
	vPID->maxdKi = 1;
	vPID->maxdKd = 1;
	vPID->mindKp = -2;
	vPID->mindKi = 0;
	vPID->mindKd = 0;
    vPID->setpoint = 42.0f;  
  
}


void PMW_OUT(uint32_t pwmvalue)
{
    pwm_on_time = ctltime*pwmvalue/PWM_MAX_VALUE;
    pwm_0ff_time = ctltime - pwm_on_time;
}

void PID_run(FUZZYPID *vPID , float pv)
{
	float thisError;
	float deltaK[3]= {0};
	thisError = vPID->setpoint - pv;
    float PIDOUT = 0;
    if(thisError > vPID->maximum)
	{
         PIDOUT = 1000;
        
	}
	else if(thisError < vPID->minimum)
	{
		 PIDOUT = 0;
	}
	else
	{
        FuzzyComputation(vPID, pv, deltaK);
		vPID->sumerror += thisError;
		vPID->output = (int)((deltaK[0] * thisError) + deltaK[1] * vPID->sumerror);
		vPID->output = vPID->output*PWM_GAIN;
        //vPID->output = (int)((vPID->kp * thisError) + vPID->ki * vPID->sumerror);
        if(vPID->output > 1000)
        {
            PIDOUT = 1000;
        }
        else if(vPID->output <=0)
        {
            PIDOUT = 0;
        }
        else 
            PIDOUT = vPID->output;
        
	}
    PMW_OUT((uint32_t)( PIDOUT +0.5f));
}
void PWM_LOOP(VOID)
{
    float pv;
   if(automodule ==0)
   {
       if(pwmoutvalue!=0)
            PWM_PIN_ON();
       else
           PWM_PIN_OFF();
       
   }       
   if((SysTick_GetTick() - lastsystime >=1) && (automodule ==1))
   { 
       lastsystime = SysTick_GetTick();
       pwmcount ++;
       
       if(pwmcount >ctltime)
       {
           pwmcount = 0;
           /*获取温度值*/
           pv = get_relvalue(0);
           PID_run(&pid, pv);
       }
       if(pwmcount < pwm_on_time)
       {
          PWM_PIN_ON();
       }
      else 
       {
           PWM_PIN_OFF();
       }
   } 
  
}